#include <TimerOne.h> //申明库文件
#include <Arduino.h>
#define left_pin   A3//左侧模拟灰度传感器
#define right_pin  A2//右侧模拟灰度传感器


#define LFwheel_1   5//左前轮引脚1
#define LFwheel_2   6//左前轮引脚2
#define RFwheel_1   9//右前轮引脚1
#define RFwheel_2   10//右前轮引脚2

int turn_panduan = 0;
int left_val=0;
int right_val=0;
int time = 0;



//上斜坡有三种状态 加速、普速与减速shangpo_high()\shangpo_forward()\shangpo_low()

//上台阶有n种状态 低速taijie_low()、高速taijie_high()、中高速（前轮上台阶后，让后轮上台阶）taijie_zhong()、低速、高速（第二个台阶）
//中高速（前轮上台阶后，让后轮上台阶）、低速、停止、低速、停止....

void Motor_Init()
{
  //初始化灰度传感器
  pinMode(left_pin,INPUT);
  pinMode(right_pin,INPUT);
  pinMode(LFwheel_1,OUTPUT);
  pinMode(LFwheel_2,OUTPUT);
  pinMode(RFwheel_1,OUTPUT);
  pinMode(RFwheel_2,OUTPUT);

}

void shangpo_high(){
  analogWrite(LFwheel_1,180);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,160);
  analogWrite(RFwheel_2,0);
}

void shangpo_low(){
  analogWrite(LFwheel_1,100);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,60);
  analogWrite(RFwheel_2,0);
}

void shangpo_low2(){
  analogWrite(LFwheel_1,30);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,10);
  analogWrite(RFwheel_2,0);
}

void shangpo_low3(){
  analogWrite(LFwheel_1,60);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,60);
  analogWrite(RFwheel_2,0);
}
void shangpo_low4(){
  analogWrite(LFwheel_1,100);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,80);
  analogWrite(RFwheel_2,0);


}

void shangpo_low5(){
  analogWrite(LFwheel_1,130);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,70);
  analogWrite(RFwheel_2,0);

}

void shangpo_back(){
  analogWrite(LFwheel_1,0);//-55
  analogWrite(LFwheel_2,80);
  analogWrite(RFwheel_1,0);
  analogWrite(RFwheel_2,60);

  
}



void taijie_low(){
  analogWrite(LFwheel_1,85);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,85);
  analogWrite(RFwheel_2,0);
}

void taijie_low2(){
  analogWrite(LFwheel_1,55);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,55);
  analogWrite(RFwheel_2,0);
}

void taijie_high(){
  analogWrite(LFwheel_1,150);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,130);
  analogWrite(RFwheel_2,0);
}

void taijie_put(){
  analogWrite(LFwheel_1,130);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,110);
  analogWrite(RFwheel_2,0);
}
void taijie_put3(){
  analogWrite(LFwheel_1,100);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,80);
  analogWrite(RFwheel_2,0);
}

void taijie_put2(){
  analogWrite(LFwheel_1,180);//-55
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,160);
  analogWrite(RFwheel_2,0);
}
void drive_forward()
{
  analogWrite(LFwheel_1,100);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,100);
  analogWrite(RFwheel_2,0);
}

void drive_forward2()
{
  analogWrite(LFwheel_1,100);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,95);
  analogWrite(RFwheel_2,0);
}

void drive_forward3()
{
  analogWrite(LFwheel_1,120);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,100);
  analogWrite(RFwheel_2,0);
}



//小车左转
void drive_left()
{
  analogWrite(LFwheel_1,30);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,160);
  analogWrite(RFwheel_2,0);
}
void drive_left2()
{
  analogWrite(LFwheel_1,0);
  analogWrite(LFwheel_2,200);
  analogWrite(RFwheel_1,220);
  analogWrite(RFwheel_2,0);
}
//小车右转
void drive_right()
{
  analogWrite(LFwheel_1,185);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,30);
  analogWrite(RFwheel_2,0);
}

void drive_right2()
{
  analogWrite(LFwheel_1,220);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,0);
  analogWrite(RFwheel_2,200);
}

void drive_back()
{
  analogWrite(LFwheel_1,0);
  analogWrite(LFwheel_2,130);
  analogWrite(RFwheel_1,130);
  analogWrite(RFwheel_2,0);
}

// 小车停止
void stop()
{
  analogWrite(LFwheel_1,255);
  analogWrite(LFwheel_2,255);
  analogWrite(RFwheel_1,255);
  analogWrite(RFwheel_2,255);
}



void trace()
{
   left_val=analogRead(left_pin);
   right_val=analogRead(right_pin);
   turn_panduan = left_val - right_val;
  //  Serial.println(left_val);
  //  Serial.println(right_val);
  //  Serial.println("");

  if (turn_panduan < -50 && turn_panduan > -150) //当右侧灰度值大于900为检测黄地 左转
  {
     drive_left();
  }
  else{
  if(turn_panduan < -150){
    drive_left2();
  }
  else{
    if (turn_panduan > 50 && turn_panduan < 150) //当左侧灰度值大于900为检测黄地 右转
    {
      drive_right(); 
    }
    else{
        if (turn_panduan > 150) //当左侧灰度值大于900为检测黄地 右转
    {
      drive_right2(); 
    }
    else{

      drive_forward();
    }
    }
  }
  }
}

void drive_forward_1(int after_time){
    for(int now_time = 0;now_time < after_time;now_time++){
      trace();
      Serial.println(now_time);
    }
}

void trace2()
{
   left_val=analogRead(left_pin);
   right_val=analogRead(right_pin);
   turn_panduan = left_val - right_val;
   Serial.print(left_val);
   Serial.print(" ");
   Serial.println(right_val);
   //Serial.println(turn_panduan);

  if (turn_panduan < -200 && turn_panduan > -300) //当右侧灰度值大于900为检测黄地 左转
  {
    drive_left();
  }
  else{
  if(turn_panduan < -300){
    drive_left2();
  }
  else{
    if (turn_panduan > 200 && turn_panduan < 300) //当左侧灰度值大于900为检测黄地 右转
    {
      drive_right(); 
    }
    else{
        if (turn_panduan > 300) //当左侧灰度值大于900为检测黄地 右转
    {
      drive_right2(); 
    }
    else{

      drive_forward();
    }
    }
  }
  }
}

void trace_forward(int after_time){
    for(int now_time = 0;now_time < after_time;now_time++){
      trace();
    }
}

